#include "ai.h"
using namespace std;

Ai::Ai(void)
{
    //ctor
}

Ai::~Ai()
{
    //dtor
}

//suporting functions

int Ai::move_size(Element *CurrentN)
{
	int x_dis;
	int move;
	if( GOAL == 0 )
	{
		x_dis = CurrentN->own_robot.getX();
	}
	else
	{
		x_dis = 1800 - CurrentN->own_robot.getX();
	}
	move = x_dis / MAX_DEPTH;
	return move;

}



float Ai::dist(int x1, int y1, int x2, int y2)
{
	return sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2));

}

float Ai::parabool(float dist)
{
	float waarde;
	waarde = 0.0051948*dist*dist -4.675324*dist + 1000;
	if( waarde < 0 )
		waarde = 0;
	return waarde;
}

void Ai::del_element(Element *CurrentN)
{
	if(CurrentN == NULL)
		return;
	if(CurrentN->depth < MAX_DEPTH)
	{
		del_element(CurrentN->next[0]);
		del_element(CurrentN->next[1]);
		del_element(CurrentN->next[2]);
		del_element(CurrentN->next[3]);
		del_element(CurrentN->next[4]);
	}
	delete(CurrentN);

}


float Ai::angle( int from_x, int from_y, int to_x, int to_y)
{
	return atan2(from_x - to_x, to_y - from_y ) * 57.2957795;
}

int Ai::check_parents(Element *CurrentN)
{
	Element *tempN;
	Element *nextN;
	int dist;

	tempN = CurrentN;

	while( tempN->parent != NULL )
	{
		if( CurrentN->depth%2 == 1)
		{
			nextN =  tempN->parent;
			dist = (CurrentN->own_robot.getX() - nextN->own_robot.getX()) *
			  	  (CurrentN->own_robot.getX() - nextN->own_robot.getX()) +
			  	  (CurrentN->own_robot.getY() - nextN->own_robot.getY()) *
			  	  (CurrentN->own_robot.getY() - nextN->own_robot.getY());

			if(dist < DIFF_DIST && CurrentN->own_robot.getRotation() == nextN->own_robot.getRotation() )
			{
				return 1;
			}
		}
		else
		{
			nextN =  tempN->parent;
			dist = (CurrentN->enemy_robot.getX() - nextN->enemy_robot.getX()) *
				  (CurrentN->enemy_robot.getX() - nextN->enemy_robot.getX()) +
				  (CurrentN->enemy_robot.getY() - nextN->enemy_robot.getY()) *
				  (CurrentN->enemy_robot.getY() - nextN->enemy_robot.getY());

			if(dist < DIFF_DIST && CurrentN->enemy_robot.getRotation() == nextN->enemy_robot.getRotation() )
			{
				return 1;
			}
		}
		tempN = nextN;
	}
	return 0;

}


void Ai::calculate_cost_a(Element *CurrentN)
{
	float angle_lw;
	float angle_lr;

	float angle_rw;
	float angle_rr;

	angle_lw = angle(CurrentN->own_robot.getX(), CurrentN->own_robot.getY(), 0, 525);
	angle_lr = angle(CurrentN->own_robot.getX(), CurrentN->own_robot.getY(), CurrentN->enemy_robot.getX(), CurrentN->enemy_robot.getY()-100);
	angle_rw = angle(CurrentN->own_robot.getX(), CurrentN->own_robot.getY(), 0, 975);
	angle_rr = angle(CurrentN->own_robot.getX(), CurrentN->own_robot.getY(), CurrentN->enemy_robot.getX(), CurrentN->enemy_robot.getY()+100);

	if( fabs(angle_lw - angle_lr) < fabs(angle_rw - angle_rr) )
	{
		CurrentN->cost =   fabs( CurrentN->own_robot.getRotation() - (fabs(angle_rw - angle_rr)/5 + angle_rw) );
	}
	else
	{
		CurrentN->cost =  fabs( CurrentN->own_robot.getRotation() - (  angle_lw - fabs(angle_lw - angle_lr)/5 ) );
	}

	CurrentN->cost +=  parabool( dist(0,750,CurrentN->own_robot.getX(),CurrentN->own_robot.getY()) );
}

void Ai::calculate_cost_d(Element *CurrentN)
{
	CurrentN->cost =  dist(1800,750,CurrentN->own_robot.getX(),CurrentN->own_robot.getY()) - dist(CurrentN->enemy_robot.getX(),CurrentN->enemy_robot.getY(),CurrentN->own_robot.getX(),CurrentN->own_robot.getY());
    CurrentN->cost += (CurrentN->enemy_robot.getRotation()-CurrentN->own_robot.getRotation())%180;
}


void Ai::adjust_angle(Element *node,int step, int flag)
{
	int hoek;

	if( flag == 0 )
	{
		hoek = 	(node->own_robot.getRotation() + step)%360;
		if( hoek < 0 )
			hoek = 360 + hoek;
		node->own_robot.setRotation( hoek );
	}
	else
	{
		hoek = 	(node->enemy_robot.getRotation() + step)%360;
		if( hoek < 0 )
			hoek = 360 + hoek;
		node->enemy_robot.setRotation( hoek);
	}
}

void Ai::adjustlocation(Element *node, int step, int flag)
{
	double x,y;
	int x_int,y_int;

	if( flag == 0 )
	{
		x = node->own_robot.getRotation();
		x = x / 57.2957795;
		x = -sin(x);
		x = x * step;
		x = x + node->own_robot.getX();

		y = node->own_robot.getRotation();
		y = y / 57.2957795;
		y = cos(y);
		y = y * step;
		y = y + node->own_robot.getY();

		x_int = (int)x;
		y_int = (int)y;

		if( x_int <= 75 )
			x_int = 75;
		if( y_int <= 75 )
			y_int = 75;

		if( x_int >= 1725 )
			x_int = 1725;
		if( y_int >= 1425 )
			y_int = 1425;



		node->own_robot.setX( x_int );
		node->own_robot.setY( y_int );

	}
	else
	{
		x = node->enemy_robot.getRotation();
		x = x / 57.2957795;
		x = -sin(x);
		x = x * step;
		x = x + node->enemy_robot.getX();

		y = node->enemy_robot.getRotation();
		y = y / 57.2957795;
		y = cos(y);
		y = y * step;
		y = y + node->enemy_robot.getY();

		x_int = (int)x;
		y_int = (int)y;

		if( x_int <= 75 )
			x_int = 75;
		if( y_int <= 75 )
			y_int = 75;

		if( x_int >= 1725 )
			x_int = 1725;
		if( y_int >= 1425 )
			y_int = 1425;

		node->enemy_robot.setX( x_int );
		node->enemy_robot.setY( y_int );
	}

}


void Ai::minimax_d(Element *CurrentN, int depth, int step)
{
	if( depth < MAX_DEPTH )
	{
		Element *tempN;


		//tempN = nieuwe locatie door met step_move vooruit te gaan
		tempN = new Element();
		tempN->parent = CurrentN;
		CurrentN->next[0]=tempN;

		tempN->own_robot.setX(  CurrentN->own_robot.getX()   );
		tempN->own_robot.setY(  CurrentN->own_robot.getY()   );
		tempN->own_robot.setRotation(  CurrentN->own_robot.getRotation()   );

		tempN->enemy_robot.setX(  CurrentN->enemy_robot.getX()   );
		tempN->enemy_robot.setY(  CurrentN->enemy_robot.getY()   );
		tempN->enemy_robot.setRotation(  CurrentN->enemy_robot.getRotation()   );

		adjustlocation( tempN, step, depth%2);
		tempN->depth = depth+1;

    	if(  check_parents(tempN) or dist(tempN->own_robot.getX(),tempN->own_robot.getY(),tempN->enemy_robot.getX(),tempN->enemy_robot.getY() ) < 220  )
		{
			CurrentN->next[CurrentN->counter]=NULL;
			CurrentN->counter++;
			delete tempN;
		}
		else
		{
        		CurrentN->counter++;
                minimax_d(tempN, depth+1, step);
        }


		//tempN = nieuwe locatie door met step_move achteruit te gaan
		tempN = new Element();
		tempN->parent = CurrentN;
		CurrentN->next[1]=tempN;

		tempN->own_robot.setX(  CurrentN->own_robot.getX()   );
		tempN->own_robot.setY(  CurrentN->own_robot.getY()   );
		tempN->own_robot.setRotation(  CurrentN->own_robot.getRotation()   );

		tempN->enemy_robot.setX(  CurrentN->enemy_robot.getX()   );
		tempN->enemy_robot.setY(  CurrentN->enemy_robot.getY()   );
		tempN->enemy_robot.setRotation(  CurrentN->enemy_robot.getRotation()   );

		adjustlocation( tempN, -step, depth%2);
		tempN->depth = depth+1;
       	if(  check_parents(tempN) or dist(tempN->own_robot.getX(),tempN->own_robot.getY(),tempN->enemy_robot.getX(),tempN->enemy_robot.getY() ) < 220  )
		{
			CurrentN->next[CurrentN->counter]=NULL;
			CurrentN->counter++;
			delete tempN;
		}
		else
		{
        		CurrentN->counter++;
			minimax_d(tempN, depth+1, step);
		}


		//tempN = nieuwe locatie door te roteren met 15°
		tempN = new Element();
		tempN->parent = CurrentN;
		CurrentN->next[2]=tempN;

		tempN->own_robot.setX(  CurrentN->own_robot.getX()   );
		tempN->own_robot.setY(  CurrentN->own_robot.getY()   );
		tempN->own_robot.setRotation(  CurrentN->own_robot.getRotation()   );

		tempN->enemy_robot.setX(  CurrentN->enemy_robot.getX()   );
		tempN->enemy_robot.setY(  CurrentN->enemy_robot.getY()   );
		tempN->enemy_robot.setRotation(  CurrentN->enemy_robot.getRotation()   );

		adjust_angle( tempN, 10, depth%2);
		tempN->depth = depth+1;
        	if(  check_parents(tempN)  )
		{
			CurrentN->next[CurrentN->counter]=NULL;
			CurrentN->counter++;
			delete tempN;
		}
		else
		{
        		CurrentN->counter++;
			minimax_d(tempN, depth+1, step);
		}



		//tempN = nieuwe locatie door te roteren met -15°
		tempN = new Element();
		tempN->parent = CurrentN;
		CurrentN->next[3]=tempN;

		tempN->own_robot.setX(  CurrentN->own_robot.getX()   );
		tempN->own_robot.setY(  CurrentN->own_robot.getY()   );
		tempN->own_robot.setRotation(  CurrentN->own_robot.getRotation()   );

		tempN->enemy_robot.setX(  CurrentN->enemy_robot.getX()   );
		tempN->enemy_robot.setY(  CurrentN->enemy_robot.getY()   );
		tempN->enemy_robot.setRotation(  CurrentN->enemy_robot.getRotation()   );

		adjust_angle( tempN, -10, depth%2);
		tempN->depth = depth+1;
        	if(  check_parents(tempN)  )
		{
			CurrentN->next[CurrentN->counter]=NULL;
			CurrentN->counter++;
			delete tempN;
		}
		else
		{
        		CurrentN->counter++;
			minimax_d(tempN, depth+1, step);
		}


		if( CurrentN->next[0] == NULL &&  CurrentN->next[1] == NULL &&  CurrentN->next[2] == NULL && CurrentN->next[3] == NULL )
		{

			calculate_cost_d(CurrentN);
			return;
		}

		//alle takken bezocht -> kiezen welke waarde naar boven te brengen
		int i;
		float large=0;
		float small=99999;

		for(i=0;i<4;i++)
		{
			if( CurrentN->depth%2 != 0 && CurrentN->next[i] != NULL )
			{
				//printf("  ->  %d\n",CurrentN->next[i]->cost);
				if( CurrentN->next[i]->cost > large )
				{
					large = CurrentN->next[i]->cost;
					CurrentN->cost = large;
					CurrentN->child = CurrentN->next[i];

				}


			}
			else if( CurrentN->depth%2 == 0 && CurrentN->next[i] != NULL )
			{
				//printf("  ->  %d\n",CurrentN->next[i]->cost);
				if( CurrentN->next[i]->cost < small )
				{
					small = CurrentN->next[i]->cost;
					CurrentN->cost = small;
					CurrentN->child = CurrentN->next[i];
				}
			}
		}
	}
	else
	{
  		calculate_cost_d(CurrentN);

	}

}




void Ai::minimax_a(Element *CurrentN, int depth, int step)
{
	if( depth < MAX_DEPTH )
	{
		Element *tempN;


		//tempN = nieuwe locatie door met step_move vooruit te gaan
		tempN = new Element();
		tempN->parent = CurrentN;
		CurrentN->next[0]=tempN;

		tempN->own_robot.setX(  CurrentN->own_robot.getX()   );
		tempN->own_robot.setY(  CurrentN->own_robot.getY()   );
		tempN->own_robot.setRotation(  CurrentN->own_robot.getRotation()   );

		tempN->enemy_robot.setX(  CurrentN->enemy_robot.getX()   );
		tempN->enemy_robot.setY(  CurrentN->enemy_robot.getY()   );
		tempN->enemy_robot.setRotation(  CurrentN->enemy_robot.getRotation()   );

		adjustlocation( tempN, step, depth%2);
		tempN->depth = depth+1;

    		if(  check_parents(tempN) or dist(tempN->own_robot.getX(),tempN->own_robot.getY(),tempN->enemy_robot.getX(),tempN->enemy_robot.getY() ) < 220  )
		{
			CurrentN->next[CurrentN->counter]=NULL;
			CurrentN->counter++;
			delete tempN;
		}
		else
		{
        		CurrentN->counter++;
			minimax_a(tempN, depth+1, step);
					}


		//tempN = nieuwe locatie door met step_move achteruit te gaan
		tempN = new Element();
		tempN->parent = CurrentN;
		CurrentN->next[1]=tempN;

		tempN->own_robot.setX(  CurrentN->own_robot.getX()   );
		tempN->own_robot.setY(  CurrentN->own_robot.getY()   );
		tempN->own_robot.setRotation(  CurrentN->own_robot.getRotation()   );

		tempN->enemy_robot.setX(  CurrentN->enemy_robot.getX()   );
		tempN->enemy_robot.setY(  CurrentN->enemy_robot.getY()   );
		tempN->enemy_robot.setRotation(  CurrentN->enemy_robot.getRotation()   );

		adjustlocation( tempN, -step, depth%2);
		tempN->depth = depth+1;
       	if(  check_parents(tempN) or dist(tempN->own_robot.getX(),tempN->own_robot.getY(),tempN->enemy_robot.getX(),tempN->enemy_robot.getY() ) < 220  )
		{
			CurrentN->next[CurrentN->counter]=NULL;
			CurrentN->counter++;
			delete tempN;
		}
		else
		{
        		CurrentN->counter++;
			minimax_a(tempN, depth+1, step);
		}


		//tempN = nieuwe locatie door te roteren met 15°
		tempN = new Element();
		tempN->parent = CurrentN;
		CurrentN->next[2]=tempN;

		tempN->own_robot.setX(  CurrentN->own_robot.getX()   );
		tempN->own_robot.setY(  CurrentN->own_robot.getY()   );
		tempN->own_robot.setRotation(  CurrentN->own_robot.getRotation()   );

		tempN->enemy_robot.setX(  CurrentN->enemy_robot.getX()   );
		tempN->enemy_robot.setY(  CurrentN->enemy_robot.getY()   );
		tempN->enemy_robot.setRotation(  CurrentN->enemy_robot.getRotation()   );

		adjust_angle( tempN, 3, depth%2);
		tempN->depth = depth+1;
        	if(  check_parents(tempN)  )
		{
			CurrentN->next[CurrentN->counter]=NULL;
			CurrentN->counter++;
			delete tempN;
		}
		else
		{
        		CurrentN->counter++;
			minimax_a(tempN, depth+1, step);
		}



		//tempN = nieuwe locatie door te roteren met -15°
		tempN = new Element();
		tempN->parent = CurrentN;
		CurrentN->next[3]=tempN;

		tempN->own_robot.setX(  CurrentN->own_robot.getX()   );
		tempN->own_robot.setY(  CurrentN->own_robot.getY()   );
		tempN->own_robot.setRotation(  CurrentN->own_robot.getRotation()   );

		tempN->enemy_robot.setX(  CurrentN->enemy_robot.getX()   );
		tempN->enemy_robot.setY(  CurrentN->enemy_robot.getY()   );
		tempN->enemy_robot.setRotation(  CurrentN->enemy_robot.getRotation()   );

		adjust_angle( tempN, -3, depth%2);
		tempN->depth = depth+1;
        	if(  check_parents(tempN)  )
		{
			CurrentN->next[CurrentN->counter]=NULL;
			CurrentN->counter++;
			delete tempN;
		}
		else
		{
        		CurrentN->counter++;
			minimax_a(tempN, depth+1, step);
		}


		if( CurrentN->next[0] == NULL &&  CurrentN->next[1] == NULL &&  CurrentN->next[2] == NULL && CurrentN->next[3] == NULL )
		{

			calculate_cost_a(CurrentN);
			return;
		}

		//alle takken bezocht -> kiezen welke waarde naar boven te brengen
		int i;
		float large=0;
		float small=99999;

		for(i=0;i<4;i++)
		{
			if( CurrentN->depth%2 != 0 && CurrentN->next[i] != NULL )
			{
				//printf("  ->  %d\n",CurrentN->next[i]->cost);
				if( CurrentN->next[i]->cost > large )
				{
					large = CurrentN->next[i]->cost;
					CurrentN->cost = large;
					CurrentN->child = CurrentN->next[i];

				}


			}
			else if( CurrentN->depth%2 == 0 && CurrentN->next[i] != NULL )
			{
				//printf("  ->  %d\n",CurrentN->next[i]->cost);
				if( CurrentN->next[i]->cost < small )
				{
					small = CurrentN->next[i]->cost;
					CurrentN->cost = small;
					CurrentN->child = CurrentN->next[i];
				}
			}
		}
	}
	else
	{
  		calculate_cost_a(CurrentN);

	}

}



Location Ai::transform(Location loc)
{
    Location temp;
    temp.setX( loc.getX() / TRANSPONEERD );
    temp.setY( loc.getY() / TRANSPONEERD );
    temp.setRotation(  round( loc.getRotation() /45) * 45  );

    return temp;
}

Location Ai::next_step(Location own, Location wanted, int flag)
{

    //variabelen
    int field[FIELD_X][FIELD_Y];
    int i,j;
    int i_start,i_end;
    int j_start,j_end;

    int wanted_x,wanted_y,wanted_rotation;
    int own_x,own_y,own_rotation;

    Location wanted_location_transformed;
    Location own_location_transformed;
    Location next_loc;

    wanted_location_transformed = transform( wanted );
    own_location_transformed = transform( own );

    //coordinaten in lokale vars steken
    wanted_x = wanted_location_transformed.getX();
    wanted_y = wanted_location_transformed.getY();
    wanted_rotation = wanted_location_transformed.getRotation();

    own_x = own_location_transformed.getX();
    own_y = own_location_transformed.getY();
    own_rotation = own_location_transformed.getRotation();


    //Filling the simplefied field with 99
    for(i=0; i<FIELD_X; i++)
    {
        for(j=0; j<FIELD_X; j++)
        {
            field[i][j] = 999;
        }
    }


    //calculation the upper left corner and lower right corner
    // upper_left = (i_start,j_start) lower right = (i_end,j_end)

    i_start = MIN(own_x,wanted_x);
    j_start = MIN(wanted_y,own_y);

    i_end = MAX(own_x,wanted_x);
    j_end = MAX(wanted_y,own_y);


    //Rechthoek arround the wanted and own location
    for(i=i_start; i<=i_end; i++)
    {
        for(j=j_start; j<=j_end; j++)
        {
            field[i][j] = 10;
        }
    }


    //The next blob of code gives a cost value arround the wanted_location
    //The code is designed to handle edge cases and give a good angle

    //checking if the upper left corner of the "rechthoek" is an wanted location

    if( flag > 0)
    {
        if( wanted_x == i_start && wanted_y == j_start )
        {
            if( (own_x - wanted_x) != 0 && (own_y - wanted_y) != 0)
            {
                field[wanted_x+1][wanted_y+1] = 10*( ABS(315 - wanted_rotation)%180 ) /45+1;          //cross right down
                    if( ABS(own_x - wanted_x) >= 3 && ABS(own_y - wanted_y) >= 3 )
                        field[wanted_x+2][wanted_y+2] = field[wanted_x+1][wanted_y+1]*field[wanted_x+1][wanted_y+1];
            }

            if( (own_x - wanted_x) != 0)
            {
                field[wanted_x+1][wanted_y] =10* (ABS(270 - wanted_rotation)%180)/45+1;           //downward
                if( ABS(own_x - wanted_x) >= 3)
                {
                    field[wanted_x+2][wanted_y-0] = field[wanted_x+1][wanted_y-0]*field[wanted_x+1][wanted_y-0];
                    field[wanted_x+2][wanted_y+1] = (field[wanted_x+2][wanted_y+2] + field[wanted_x+2][wanted_y-0])/2+1;
                }
            }

            if( (own_y - wanted_y) != 0)
            {
                field[wanted_x-0][wanted_y+1] = 10*(ABS(0 - wanted_rotation)%180)/45+1;          //right sideways
                if( ABS(own_y - wanted_y) >= 3)
                {
                    field[wanted_x-0][wanted_y+2] = 10*field[wanted_x-0][wanted_y+1]*field[wanted_x-0][wanted_y+1];
                    field[wanted_x+1][wanted_y+2] = (field[wanted_x+2][wanted_y+2] + field[wanted_x-0][wanted_y+2])/2+1;
                }
            }

        }
        //cheking if the lower left corner of the "rechthoek" is an wanted location
        else if( wanted_x == i_end && wanted_y == j_start)
        {
            if( (own_x - wanted_x) != 0 && (own_y - wanted_y) != 0)
            {
                field[wanted_x-1][wanted_y+1] = 10*( ABS(135 - wanted_rotation)%180 ) /45+1;          //cross right up
                if( ABS(own_x - wanted_x) >= 3 && ABS(own_y - wanted_y) >= 3 )
                    field[wanted_x-2][wanted_y+2] = field[wanted_x-1][wanted_y+1]*field[wanted_x-1][wanted_y+1];
            }

            if( (own_x - wanted_x) != 0)
            {
                field[wanted_x-1][wanted_y-0] = 10*(ABS(90 - wanted_rotation)%180)/45+1;           //upward
                if( ABS(own_x - wanted_x) >= 3)
                {
                    field[wanted_x-2][wanted_y-0] = field[wanted_x-1][wanted_y-0]*field[wanted_x-1][wanted_y-0];
                    field[wanted_x-2][wanted_y+1] = (field[wanted_x-2][wanted_y+2] + field[wanted_x-2][wanted_y-0])/2+2;
                }
            }

            if( (own_y - wanted_y) != 0)
            {
                field[wanted_x-0][wanted_y+1] = 10*(ABS(180 - wanted_rotation)%180)/45+1;          //right sideways
                if( ABS(own_y - wanted_y) >= 3)
                {
                    field[wanted_x-0][wanted_y+2] = field[wanted_x-0][wanted_y+1]*field[wanted_x-0][wanted_y+1];
                    field[wanted_x-1][wanted_y+2] = (field[wanted_x-2][wanted_y+2] + field[wanted_x-0][wanted_y+2])/2+2;
                }
            }
        }
        //cheking if the upper right corner of the "rechthoek" is an wanted location
        else if( wanted_x == i_start && wanted_y == j_end)
        {
            if( (own_x - wanted_x) != 0 && (own_y - wanted_y) != 0)
            {
                field[wanted_x+1][wanted_y-1] = 10*( ABS(225 - wanted_rotation)%180 ) /45+1;          //cross left down
                if( ABS(own_x - wanted_x) >= 3 && ABS(own_y - wanted_y) >= 3 )
                    field[wanted_x+2][wanted_y-2] = field[wanted_x+1][wanted_y-1]*field[wanted_x+1][wanted_y-1];
            }
            if( (own_x - wanted_x) != 0)
            {
                field[wanted_x+1][wanted_y-0] = 10*(ABS(270 - wanted_rotation)%180)/45+1;           //downard
                if( ABS(own_x - wanted_x) >= 3)
                {
                    field[wanted_x+2][wanted_y-0] = field[wanted_x+1][wanted_y-0]*field[wanted_x+1][wanted_y-0];
                    field[wanted_x+2][wanted_y-1] = (field[wanted_x+2][wanted_y-2] + field[wanted_x+2][wanted_y-0])/2+1;
                }
            }
            if( (own_y - wanted_y) != 0)
            {
                field[wanted_x-0][wanted_y-1] = 10*(ABS(180 - wanted_rotation)%180)/45+1;          //left sideways
                if( ABS(own_y - wanted_y) >= 3)
                {
                    field[wanted_x-0][wanted_y-2] = field[wanted_x-0][wanted_y-1]*field[wanted_x-0][wanted_y-1];
                    field[wanted_x+1][wanted_y-2] = (field[wanted_x+2][wanted_y-2] + field[wanted_x][wanted_y-2])/2+1;
                }
            }

        }
        //cheking id the lower right corner of the "rechthoek" is an wanted location
        else if ( wanted_x == i_end && wanted_y == j_end)
        {
            if( (own_x - wanted_x) != 0 && (own_y - wanted_y) != 0)
            {
                field[wanted_x-1][wanted_y-1] = 10*( ABS(135 - wanted_rotation)%180 ) /45+1;          //cross left up
                if( ABS(own_x - wanted_x) >= 3 && ABS(own_y - wanted_y) >= 3 )
                    field[wanted_x-2][wanted_y-2] = field[wanted_x-1][wanted_y-1]*field[wanted_x-1][wanted_y-1];
            }

            if( (own_x - wanted_x) != 0)
            {
                field[wanted_x-1][wanted_y-0] = 10*(ABS(90 - wanted_rotation)%180)/45+1;           //upward
                if( ABS(own_x - wanted_x) >= 3)
                {
                    field[wanted_x-2][wanted_y-0] = field[wanted_x-1][wanted_y-0]*field[wanted_x-1][wanted_y-0];
                    field[wanted_x-2][wanted_y-1] = (field[wanted_x-2][wanted_y-2] + field[wanted_x-2][wanted_y])/2+1;
                }
            }
            if( (own_y - wanted_y) != 0)
            {
                field[wanted_x-0][wanted_y-1] = 10*(ABS(180 - wanted_rotation)%180)/45+1;          //left sideways
                if( ABS(own_y - wanted_y) >= 3)
                {
                    field[wanted_x-0][wanted_y-2] = field[wanted_x-0][wanted_y-1]*field[wanted_x-0][wanted_y-1];
                    field[wanted_x-1][wanted_y-2] = (field[wanted_x-2][wanted_y-2] + field[wanted_x][wanted_y-2])/2+1;
                }
            }

        }

    }
    //create large matrix
    //fill it with pointers to nodes we are going to create next
    //in a next step you can link them together
    Node *field_pointers[FIELD_X][FIELD_Y];
    for(i=0; i<FIELD_X; i++)
    {
        for(j=0; j<FIELD_X; j++)
        {
            field_pointers[i][j] = NULL;
        }
    }

    //create new nodes for the working rectangle
    for(i=i_start;i<=i_end;i++)
    {
        for(j=j_start;j<=j_end;j++)
        {
            field_pointers[i][j] = new Node(i,j,field[i][j]);
        }
    }

    //link the nodes in the working rectangle togheter in all directions: left, right, up, ...
    for(i=i_start;i<=i_end;i++)
    {
        for(j=j_start;j<=j_end;j++)
        {
            //up
            if( field[i-1][j] != 999 && i != i_start )
            {
                field_pointers[i][j]->up = field_pointers[i-1][j];
            }

            //down
            if( field[i+1][j] != 999 && i != i_end)
            {
                field_pointers[i][j]->down = field_pointers[i+1][j];
            }

            //right
            if( field[i][j+1] != 999 && j != j_end)
            {
                field_pointers[i][j]->right = field_pointers[i][j+1];
            }

            //left
            if( field[i][j-1] != 999 && j != j_start )
            {
                field_pointers[i][j]->left = field_pointers[i][j-1];
            }

            //left up
            if( field[i-1][j-1] != 999 && i != i_start && j != j_start)
            {
                field_pointers[i][j]->left_up = field_pointers[i-1][j-1];
            }

            //left down
            if( field[i+1][j-1] != 999 && i != i_end && j != j_start)
            {
                field_pointers[i][j]->left_down = field_pointers[i+1][j-1];
            }


            //right down
            if( field[i+1][j+1] != 999 && i != i_end && j != j_end)
            {
                field_pointers[i][j]->right_down = field_pointers[i+1][j+1];
            }

             //right up
            if( field[i-1][j+1] != 999 && i != i_start && j != j_end)
            {
                field_pointers[i][j]->right_up = field_pointers[i-1][j+1];
            }
        }
    }



    //dijkstra algorimte
    int lowcost;
    int open_index=0;
    int current_index;
    int flagend;

    Node *currentN;
    Node *tempN;

    Node *open_list[FIELD_X*FIELD_Y];
    for(i=0;i<FIELD_X*FIELD_Y;i++)
    {
        open_list[i] = NULL;
    }


    //step1
    //Add the starting node to the open list.
    currentN = field_pointers[own_x][own_y];
    currentN->cost = 0;
    currentN->open = 1;
    currentN->prev = NULL;
    open_list[open_index]=currentN;
    open_index++;

    //step 2
    //repeat the following
    while(1)
    {

        //step 2.a
        //Look for the lowest cost node on the open list.
        //We refer to this as the current node.
        lowcost = 999;
        flag = 0;
        for(i=0;i<open_index;i++)
        {
            if( open_list[i] != NULL )
            {
                if( lowcost > open_list[i]->cost )
                {
                    lowcost = open_list[i]->cost;
                    current_index = i;
                    flagend = 1;
                }
            }
        }

        //Step 5
        //stop if there are no more open nodes
        //The shortest path from each node to the origin can be found using the backtracking prev pointer.
        if( flagend == 0 )
        {
            break;
        }
        currentN = open_list[current_index];


        //step 2.b
        //set current node as visited and remove from open list
        open_list[current_index] = NULL;
        currentN->visited = 1;
        currentN->open = 0;


        //step2.c
        //For each of the 8 nodes adjacent to the current node
        //If it is not walkable or if it is on the closed list, ignore it. Otherwise do the following.
        //If it isn’t on the open list, add it to the open list. Make the current node the parent of this square. Record the cost.
        //If it is on the open list already, check to see if recent calculated cost is lower.If so, change the parent of the node
        //to the current square, and overwrite with the lower cost.
        //add an extra cost of 4 for diaganol directions.


        tempN = currentN->up;
        if( tempN != NULL )
        {
            if(tempN->visited != 1)
            {

                //not on the open list
                if( tempN->open == 0 )
                {

                    //add to the open list
                    open_list[open_index] = tempN;
                    tempN->index = open_index;
                    open_index++;
                    tempN->prev = currentN;
                    tempN->cost = tempN->path_cost + currentN->cost;
                    tempN->open = 1;
                    tempN->rotation = 90;
                }
                else //if( tempN->open == 1 )
                {
                    if(  open_list[tempN->index]->cost > (tempN->path_cost + currentN->cost) )
                    {
                        open_list[tempN->index]->cost = tempN->path_cost + currentN->cost;
                        open_list[tempN->index]->prev = currentN;
                        open_list[tempN->index]->rotation = 90;
                    }
                }

            }
        }

        tempN = currentN->down;
        if( tempN != NULL )
        {
            if(tempN->visited != 1)
            {
                //not on the open list
                if( tempN->open == 0 )
                {
                    //add to the open list
                    open_list[open_index] = tempN;
                    tempN->index = open_index;
                    open_index++;
                    tempN->prev = currentN;
                    tempN->cost = tempN->path_cost + currentN->cost;
                    tempN->open = 1;
                    tempN->rotation = 270;
                }
                else //if( tempN->open == 1 )
                {
                    if(  open_list[tempN->index]->cost > (tempN->path_cost + currentN->cost) )
        			{
                        open_list[tempN->index]->cost = tempN->path_cost + currentN->cost;
                        open_list[tempN->index]->prev = currentN;
                        open_list[tempN->index]->rotation = 270;
                    }
                }
            }
        }

        tempN = currentN->left_up;
        if( tempN != NULL )
        {
            if(tempN->visited != 1)
            {
                //not on the open list
                if( tempN->open == 0 )
                {
                    //add to the open list
                    open_list[open_index] = tempN;
                    tempN->index = open_index;
                    open_index++;
                    tempN->prev = currentN;
                    tempN->cost = tempN->path_cost + currentN->cost;
                    tempN->open = 1;
                    tempN->rotation = 135;
                }
                else //if( tempN->open == 1 )
                {
                    if(  open_list[tempN->index]->cost > (tempN->path_cost + currentN->cost) )
        			{
                        open_list[tempN->index]->cost = tempN->path_cost + currentN->cost;
                        open_list[tempN->index]->prev = currentN;
                        open_list[tempN->index]->rotation = 135;
                    }
                }

            }
        }

        tempN = currentN->right_down;
        if( tempN != NULL )
        {
            if(tempN->visited != 1)
            {
                //not on the open list
                if( tempN->open == 0 )
                {
                    //add to the open list
                    open_list[open_index] = tempN;
                    tempN->index = open_index;
                    open_index++;
                    tempN->prev = currentN;
                    tempN->cost = tempN->path_cost + currentN->cost;
                    tempN->open = 1;
                    tempN->rotation = 315;
                }
                else //if( tempN->open == 1 )
                {
                    if(  open_list[tempN->index]->cost > (tempN->path_cost + currentN->cost) )
                    {
                        open_list[tempN->index]->cost = tempN->path_cost + currentN->cost;
                        open_list[tempN->index]->prev = currentN;
                        open_list[tempN->index]->rotation = 315;
                    }
                }

            }
        }

        tempN = currentN->left_down;
        if( tempN != NULL )
        {
            if(tempN->visited != 1)
            {
                //not on the open list
                if( tempN->open == 0 )
                {
                    //add to the open list
                    open_list[open_index] = tempN;
                    tempN->index = open_index;
                    open_index++;
                    tempN->prev = currentN;
                    tempN->cost = tempN->path_cost + currentN->cost;
                    tempN->open = 1;
                    tempN->rotation = 215;
                }
                else //if( tempN->open == 1 )
                {
                    if(  open_list[tempN->index]->cost > (tempN->path_cost + currentN->cost) )
        			{
                        open_list[tempN->index]->cost = tempN->path_cost + currentN->cost;
                        open_list[tempN->index]->prev = currentN;
                        open_list[tempN->index]->rotation = 215;
                    }
                }

            }
        }

        tempN = currentN->left;
        if( tempN != NULL )
        {
            if(tempN->visited != 1)
            {
                //not on the open list
                if( tempN->open == 0 )
                {
                    //add to the open list
                    open_list[open_index] = tempN;
                    tempN->index = open_index;
                    open_index++;
                    tempN->prev = currentN;
                    tempN->cost = tempN->path_cost + currentN->cost;
                    tempN->open = 1;
                    tempN->rotation = 180;
                }
                else //if( tempN->open == 1 )
                {
                    if(  open_list[tempN->index]->cost > (tempN->path_cost + currentN->cost) )
        			{
                        open_list[tempN->index]->cost = tempN->path_cost + currentN->cost;
                        open_list[tempN->index]->prev = currentN;
                        open_list[tempN->index]->rotation = 180;
                    }
                }

            }
        }

        tempN = currentN->right;
        if( tempN != NULL )
        {
            if(tempN->visited != 1)
            {
                //not on the open list
                if( tempN->open == 0 )
                {
                    //add to the open list
                    open_list[open_index] = tempN;
                    tempN->index = open_index;
                    open_index++;
                    tempN->prev = currentN;
                    tempN->cost = tempN->path_cost + currentN->cost;
                    tempN->open = 1;
                    tempN->rotation = 0;
                }
                else //if( tempN->open == 1 )
                {
                    if(  open_list[tempN->index]->cost > (tempN->path_cost + currentN->cost) )
                    {
                        open_list[tempN->index]->cost = tempN->path_cost + currentN->cost;
                        open_list[tempN->index]->prev = currentN;
                        open_list[tempN->index]->rotation = 0;
                    }
                }

            }
        }

	}

    //dijkstra algorimte has finished




    //backtrack from wanted location to start location
    //remember the second node. (=first step)
    tempN = NULL;
    currentN = field_pointers[wanted_x][wanted_y];
    while( currentN != NULL)
    {
        currentN->next= tempN;
        tempN = currentN;
		currentN = currentN->prev;

    }
    currentN = currentN->next;


    //the X and Y coördinate needs to be adjusted the real pleayfield

    next_loc.setX( currentN->co_x * TRANSPONEERD + wanted.getX() - wanted_x);
    next_loc.setY( currentN->co_y * TRANSPONEERD + wanted.getY() - wanted_y);
    next_loc.setRotation(  currentN->rotation + wanted.getRotation() - wanted_rotation );

    return next_loc;



}

